//=============================================================================
//     _   _                               ____                _  _          
//    | | | |  __ _  _ __   _ __   _   _  / ___|  _ __   _ __ (_)| |_   ___ 
//    | |_| | / _` || '_ \ | '_ \ | | | | \___ \ | '_ \ | '__|| || __| / _ \
//    |  _  || (_| || |_) || |_) || |_| |  ___) || |_) || |   | || |_ |  __/
//    |_| |_| \__,_|| .__/ | .__/  \__, | |____/ | .__/ |_|   |_| \__| \___|
//                  |_|    |_|     |___/         |_|                         
//
//                     HappySprite - We make sprites happy
//
// Copyright (c) 2007 by Tank Monkey Games
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
//=============================================================================

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Text;
using Microsoft.Xna.Framework;

namespace HappySprite.Simulation
{
    internal class PenetrationDepthSolver
    {
        struct Edge
        {
            class EdgeComparer : IComparer<Edge>
            {
                public int Compare(Edge x, Edge y)
                {
                    return x.LengthSquared.CompareTo(y.LengthSquared);
                }
            }

            static EdgeComparer comparer = new EdgeComparer();

            internal Vector2 Vertex0;
            internal Vector2 Vertex1;
            internal Vector2 Vector; // The shortest vector to the origin
            internal float LengthSquared;

            internal Edge(Vector2 vertex0, Vector2 vertex1)
            {
                Vertex0 = vertex0;
                Vertex1 = vertex1;
                Vector = Vector2.Zero;
                LengthSquared = 0;
            }

            internal static IComparer<Edge> Comparer
            {
                get { return comparer; }
            }
            
            internal bool ComputeVector()
            {
                Vector2 v = Vertex1 - Vertex0;
                Vector2 w = -Vertex0;

                float c1 = Vector2.Dot(w, v);

                if (c1 < 0)
                    return false; // ignore

                float c2 = Vector2.Dot(v, v);

                if (c2 < c1)
                    return false; // ignore

                float b = c1 / c2;

                Vector = b * v + Vertex0;
                LengthSquared = Vector.LengthSquared();

                return true;
            }
        }

        PriorityQueue<Edge> queue = new PriorityQueue<Edge>(Edge.Comparer);
        float maxLengthSquared = 0f;

        internal float MaxLengthSquared
        {
            get { return maxLengthSquared; }
        }

        internal Vector2 MinimumPenetration
        {
            get { return queue.Peek().Vector; }
        }

        internal Vector2 EdgeNormal
        {
            get
            {
                Edge edge = queue.Peek();
                Vector2 v = edge.Vertex1 - edge.Vertex0;
                return Vector2.Normalize(new Vector2(v.Y, -v.X));
            }
        }

        internal void Initialize(IEnumerable<Vector2> points)
        {
            queue.Clear();
            int count = 0;
            Vector2 last = Vector2.Zero;
            Vector2 first = Vector2.Zero;

            foreach (Vector2 point in points)
            {
                if (count == 0)
                    first = point;

                if (count++ > 0)
                    AddEdge(last, point);

                last = point;
            }

            Debug.Assert(count > 0);

            AddEdge(last, first);
        }

        internal void AddSupportPoint(Vector2 w)
        {
            // If w is a point on the edge ends, then don't spilt
            // this should cause the algorithm to terminate
            Edge edge = queue.Peek();

            if (edge.Vertex0 == w || edge.Vertex1 == w)
                return;

            edge = queue.Pop();

            AddEdge(edge.Vertex0, w);
            AddEdge(w, edge.Vertex1);

            float dot = Vector2.Dot(edge.Vector, w);
            maxLengthSquared = dot * dot / edge.LengthSquared;
        }

        void AddEdge(Vector2 pointA, Vector2 pointB)
        {
            Edge edge = new Edge(pointA, pointB);

            if (edge.ComputeVector())
            {
                queue.Push(edge);
            }
        }
    }
}
